#include <iostream>
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>


#include "constants.h"
#include "other_functions.h"

#include "view.h"
#include "model.h"

#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"


void CView2D::add_trackpoint(int i,  IplImage * img)
{
    CvRect patch;
    IplImage *imgpatch;
    CvPoint pt0;

    pt0 = Point(i);
    patch = PointRect(i);
    cvSetImageROI(img, patch);
    imgpatch = cvCreateImage(matchsizes[0], IPL_DEPTH_8U, 1);
    cvCopy(img, imgpatch);
    patches.push_back(imgpatch);
    imgpatch = cvCreateImage(matchsizes[0], IPL_DEPTH_8U, 1);
    cvCopy(img, imgpatch);
    lastpatches.push_back(imgpatch);
    imgpatch = cvCreateImage(matchsizes[0], IPL_DEPTH_8U, 1);
    cvCopy(img, imgpatch);
    newpatches.push_back(imgpatch);    
}

void CView2D::init_track(IplImage * img)
{
    int i;
    int ini = patches.capacity();

    matchsizes[0] = cvSize(51, 51);
    matchsizes[1] = cvSize(31, 31);
    matchsizes[2] = cvSize(17, 17);

    for (i=0; i<ini; i++)
	update_trackpoint(i, img, true);

    oldpose_track = *this;
    patches.set_capacity(npoints);
    newpatches.set_capacity(npoints);
    lastpatches.set_capacity(npoints);
    if (matchvals)
    {
	cvReleaseMat(&matchvals);
	cvReleaseMat(&matchpatchused);
    }
    matchvals = cvCreateMat(npoints,1,CV_32FC1);
    matchpatchused = cvCreateMat(npoints,1,CV_8UC1);

    for (i=ini; i<npoints; i++)
	add_trackpoint(i,  img);

    cvResetImageROI(img);
    
}

void CView2D::tracker(IplImage * img, CvSize size)
{
    int i=0;
    CvPoint point1, point2;
    float val;

    for (i=0; i<npoints; i++)
    {
	if (!cvGetReal1D(mask_show, i))
	    continue;

	point1 = Point(i);
	point2 = trackpoint(point1, img, patches[i], size, &val);
	//si el tracking del punto no ha dado resultado.
	if (point2.x != -1)
	{
	    cvSetReal1D(matchpatchused, i, 1);
	}
	else
	{
	    point2 = trackpoint(point1, img, newpatches[i], size, &val);
	    if (point2.x != -1)
	    {	    
		cvSetReal1D(matchpatchused, i, 2);
	    }
	    else
	    {
		point2 = trackpoint(point1, img, lastpatches[i], size, &val);
		if (point2.x != -1)
		{
		    cvSetReal1D(matchpatchused, i, 3);
		}
		else
		{
		    //eliminamos el punto de los visibles
		    cvSetReal1D(mask_show, i, 0);
		    cvSetReal1D(matchpatchused, i, 4);
		    continue;
		}
	    }
	}
	cvmSet(matchvals, i,0,val );
	cvmSet(points2d, i,0, point2.x);
	cvmSet(points2d, i,1, point2.y);
    }    
}

void CView2D::update_track(IplImage *img)
{
    int i;
    float dist = distanceR(oldpose_track);
    bool updatenew = (dist > MAX_DIST_R2);

    for (i=0; i<npoints; i++)
	update_trackpoint(i, img, updatenew);

    if (updatenew)
    {
	printf("Updated patches...............\n");
	oldpose_track = *this;
    }	
    cvResetImageROI(img);

}


void CView2D::update_trackpoint(int i, IplImage *img, bool updatenew)
{
    CvRect patch;

    patch = PointRect(i);

    cvSetImageROI(img, patch);
    cvCopy(img, lastpatches[i]);
    if (updatenew)
	cvCopy(img, newpatches[i]);
}

void CView2D::display_track(IplImage * img_track, IplImage * display)
{
    char buffer[100];
    CvRect roi;
    int w = display->width;
    //int h = display->height;
    int sz = 15;
    CvPoint p;
    CvRect patch;

    patch = cvRect(matchsizes[0].width/2-sz, matchsizes[0].height/2-sz, sz*2, sz*2);

    roi = cvRect(0,0,img_track->width, img_track->height);
    cvSetImageROI(display, roi);
    cvConvertImage(img_track, display);
    cvResetImageROI(display);
    for(int i=0; i<npoints; i++)
    {
	p = Point(i);
	cvRectangle(display, cvPoint(p.x-sz, p.y-sz), cvPoint(p.x+sz, p.y+sz), DEPTH_COLOR);
	sprintf(buffer, "%u", i);
	cvPutText(display, buffer, cvPoint(p.x-sz+1, p.y+sz-6 ), &fontmod, CV_RGB(200,100,0));
	cvPutText(display, buffer, cvPoint(w-sz*8, sz*2*(i+1)-3 ), &fontmod, CV_RGB(200,100,0));
	
    }
    for(int i=0; i<npoints; i++)
    {
	roi = cvRect(w-sz*2, sz*2*i, sz*2, sz*2);
	cvSetImageROI(display, roi);
	cvSetImageROI(patches[i], patch);
	cvConvertImage(patches[i], display);
	cvResetImageROI(patches[i]);
	roi.x -= sz*2;
	cvSetImageROI(display, roi);
	cvSetImageROI(newpatches[i], patch);
	cvConvertImage(newpatches[i], display);
	cvSetImageROI(display, roi);
	cvResetImageROI(newpatches[i]);
	roi.x -= sz*2;
	cvSetImageROI(display, roi);
	cvSetImageROI(lastpatches[i], patch);
	cvConvertImage(lastpatches[i], display);
	cvSetImageROI(display, roi);
	cvResetImageROI(lastpatches[i]);
    }
    cvResetImageROI(display);
}

CvRect CView2D::PointRect(int i)
{
    return PointRect(Point(i));
}

CvRect CView2D::PointRect(CvPoint pt0)
{
    return cvRect(pt0.x-matchsizes[0].width/2, pt0.y-matchsizes[0].height/2, 
		  matchsizes[0].width, matchsizes[0].height);
}

CvPoint CView2D::trackpoint(CvPoint pt1, IplImage *img, IplImage * oldimg, CvSize size, float * val)
{
    int method = 5;
    int methods[6] = { CV_TM_SQDIFF, CV_TM_SQDIFF_NORMED, CV_TM_CCORR, 
		       CV_TM_CCORR_NORMED, CV_TM_CCOEFF, CV_TM_CCOEFF_NORMED};
    CvPoint minpos, maxpos;
    double min, max;

    CvRect roimatch, roipatch;
    CvSize match;
    CvPoint pt0 = cvPoint(matchsizes[0].width/2, matchsizes[0].height/2);

    //tamaño de la zona de búsqueda. Tamaño del resultado
    int w = size.width;
    int h = size.height;

    if (!result)
    {
	result = cvCreateImage(cvSize(w, h), IPL_DEPTH_32F, 1);
	suma  = cvCreateMat(h, w, CV_32FC1);
    }
     cvResetImageROI(result);
    cvZero(suma);
    
    //se eligen varios tamaños de parche
    unsigned int npatches = sizeof(matchsizes)/sizeof(matchsizes[0]);
    for (unsigned int m=0; m<npatches; m++)
    {
	match = matchsizes[m];
	cvZero(result);
	
	//selección del parche a buscar
	roipatch = cvRect(pt0.x-match.width/2, pt0.y-match.height/2, match.width, match.height);
	// matching
	// selección de la ventana de matching. parche en donde buscar
	roimatch.width = roipatch.width + w -1;
	roimatch.height = roipatch.height + h -1;
	roimatch.x = pt1.x - w/2 - (int)roipatch.width/2;
	roimatch.y = pt1.y - h/2 - (int)roipatch.height/2;
	if (roimatch.y<0 || roimatch.x<0)
	{
	    break;
	}
	//selección del parche a buscar (input)
	cvSetImageROI(oldimg, roipatch);
	// selección de las venetanas de búsqueda
	cvSetImageROI(img, roimatch);
	
	cvMatchTemplate(img, oldimg, result, methods[method]);
	
	cvAdd(suma, result, suma);
	
	cvResetImageROI(img);
	cvResetImageROI(oldimg);
    }
	
    if (roimatch.y<0 || roimatch.x<0)
    {
	printf("Point not valid. Out of range\n" );
	//el punto no es válido
	return cvPoint(-1, -1);
    }
	
    //para encontrar el punto exacto, busca en máximo de la suma de las correspondencias
    // de los tres parches. Y entorno a ese máximo, busca el máximo del parche más pequeño.
    //búsqueda del máximo de los tres parches
    cvMinMaxLoc(suma, &min, &max, &minpos, &maxpos);
    // si se usa uno de los métodos de CV_TM_SQDIFF o CV_TM_SQDIFF_NORMED, se buscar el mínimo
    if (method<=1)
    {
	maxpos = minpos; 
	max = 1-min;
    }
	
    //evalua que el valor de correlación encontrado sea superior a un umbral
    if (max < MIN_CORR_VALUE*(float)npatches )
    {
	printf("Point not valid (value %f en %d).\n", max/(float)npatches, maxpos.x);
	//el punto no es válido
	return cvPoint(-1, -1);
    }
    *val = max;

    //ahora busca el máximo el parche más pequeño, en torno al punto indicado.
    // result ya contiene la correlación del parche más pequeño
    if (maxpos.x + 10 >= w) 
	maxpos.x = w-11;
    if (maxpos.x < 10)
	maxpos.x = 10;
    if (maxpos.y + 10 >= h) 
	maxpos.y = h-11;
    if (maxpos.y < 10)
	maxpos.y = 10;

    cvSetImageROI(result, cvRect(maxpos.x-5, maxpos.y-5, 10, 10));
    cvMinMaxLoc(result, &min, &max, &minpos, &maxpos);
    if (method<=1) maxpos = minpos; 

    maxpos.x += result->roi->xOffset - w/2 + pt1.x;
    maxpos.y += result->roi->yOffset - h/2 + pt1.y;

    return maxpos;
}
